/*
 * Robot.java -
 * Copyright (c) 2005 Carmine Lia
 * e-mail:      carmine.lia@libero.it
 * web-address: http://www.intermedia.sa.it/lia

 *  This program is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU General Public License
 *  as published by the Free Software Foundation; either version 2
 *  of the License, or any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */
package javax.robotics.engine.robots;

import java.io.Serializable;

import javax.robotics.vecmath.Matrix;
import javax.robotics.vecmath.RMatrix4d;
import javax.robotics.vecmath.RVector3d;
import javax.robotics.vecmath.RVector;
import javax.robotics.xml.XmlRobot;




/**
 * <p>
 * This is an interface for kinematics and dynamics model of different open chain robot manipulators.
 * </p>
 * 
 * <b>Example of use</b>
 * <p> 
 * The example <a href="doc-files/Demo.txt">Demo.java</a> shows how to use Robot class in your java based
 * application.
 * <br>
 * <!--
 * copy these lines in a text file named <code><b>Demo.java</b></code>;
 * <br>
 * compile it:
 * <br>
 * &nbsp;&nbsp;&nbsp;<code>javac -cp &lt;jar_file_path&gt;/JRoboOp.jar Demo.java</code><br>
 * execute it:
 * <br>
 * &nbsp;&nbsp;&nbsp;<code>java -Djava.library.path=&lt;dll_path&gt; -cp &lt;jar_file_path&gt;/JRoboOp.jar:. Demo</code>
 * </p>
 * -->
 * @author <a href="http://www.intermedia.sa.it/lia/">Carmine Lia</a>
 * @since 1.0.0
 * @version 16/11/2005
 */
public interface Robot extends Serializable
{
    
    boolean DEBUG = false;
    int INITROBOT_DIM = 19;
    int INITMOTOR_DIM = 4;
    int JACOBIAN_DIMENSION = 6;
    int HOMOGENEOUS_MATRIX_DIM = 16;
    int SPACE_DIM = 3;

    // ------------- Constructors
    /**
     * Gets Robot class ID
     * 
     * @return ID
     */
    short getID();
  
   
    /**
     * Gets the robot configuration.
     * @return the robot configuration.
     */
    XmlRobot getRobotConfiguration();
    
    /*
     * Robot Variables
     */
    /**
     * Gets the degrees of freedom of robot.
     * 
     * @return degrees of freedom of robot manipulator
     */
    int getDof();
    
    /**
     * Gets the available degrees of freedom of robot.
     * 
     * @return the available degrees of freedom of robot manipulator
     */
    int getAvailableDof();
    /*
     * Getters for joint variables
     */
    /**
     * Gets joints maximum position.
     * 
     * @return vector of joints maximum position.
     */
    RVector getQMax();
    
    /**
     * Gets joints minimum position.
     * 
     * @return vector of joints minimum position.
     */
    RVector getQMin();
    
    
    /**
     * Gets joints offset position.
     * 
     * @return vector of joints offset position.
     */
    RVector getQOffset();
    
    /**
     * Gets joints position.
     * The position is computed subtracting the offset.
     * @return vector of joints position.
     */
    RVector getQ();
    

    /**
     * Position of i-th joint.
     * The position is computed subtracting the offset.
     * @param i
     *            joint
     * @return joint position.
     */
    double getQ( int i);
   

    /**
     * Gets joints velocity.
     * @return vector of joints velocity
     */
    RVector getQp();
   

    /*
     * Gets velocity of i-th joint<br>
     * Not implemented in ROBOOP.
     * 
     * <p>
     * </p>
     * 
     * @param i
     *            joint
     * @return join velocity
     *
    public double getQp(int i);
    */

    /**
     * Gets joints acceleration.
     * @return vector of joints acceleration
     */
    RVector getQpp();
   

    /*
     * Gets acceleration of i-th joint<br>
     * Not implemented in ROBOOP.
     * 
     * <p>
     * </p>
     * 
     * @param i
     *            joint
     * @return join acceleration
     *
    public double getQpp(int i);
   */
    /*
     * Setters for joint variables
     */
    /**
     * Sets joints position.
     * The position is computed adding the offset.
     * @param q
     *            vector of joints position
     */
    void setQ( RVector q);
    /**
     * Sets joints position.
     * The position is computed adding the offset.
     * @param q
     *            vector of joints position
     */
    void setQ( double[] q);
    /**
     * Sets the position pf i-th joint position.
     * The position is computed adding the offset.
     * @param q
     *            joint position
     * @param i
     *            joint
     */
    void setQ( double q,  int i);
    
    /**
     * Sets joints velocity.
     * @param qp
     *            vector of joints velocity
     */
    void setQp( RVector qp);
    /**
     * Sets joints velocity.
     * @param qp
     *            array of joints velocity
     */
    void setQp( double[] qp);
    
    /*
     * Sets i-th joint velocity<br>
     * Not implemented in ROBOOP.
     * 
     * <p>
     * </p>
     * 
     * @param qp
     *            joint velocity
     * @param i
     *            joint
     *
    public void setQp(double qp, int i);
    */
    /**
     * Sets joints acceleration.
     * @param qpp
     *            vector of joints position
     */
    void setQpp( RVector qpp);
    /**
     * Sets joints acceleration.
     * @param qpp
     *            the array of joints position
     */
    void setQpp( double[] qpp);
    /*
     * Set i-th joint acceleration<br>
     * Not implemented in ROBOOP.
     * 
     * <p>
     * </p>
     * 
     * @param qpp
     *            joint acceleration
     * @param i
     *            joint
     *
    public void setQpp(double qpp, int i);
    */
    /*
     * ----------------------------- Robot Kinematics
     */
    /**
     * Computes the solution of the Inverse Kinematics Problem from homogeneous
     * transform matrix using a Newton-Raphson technique.
     * 
     * @param Tobj
     *            homogeneous transform matrix
     * @return joints position
     */
    RVector invKine( RMatrix4d Tobj);
    
    /**
     * Computes the solution of the Inverse Kinematics Problem from homogeneous
     * transform matrix using a Newton-Raphson or Taylor expansion techniques.
     * 
     * @param Tobj
     *            homogeneous transform matrix
     * @param mj
     *            value 0,1 to choose the technique: Newton-Raphson, Taylor
     *            expansion respectively.
     * @param endlink
     *            the link to pretend is the end effector.
     * @return joints position if converge otherwise null value.
     */
    RVector invKine( RMatrix4d Tobj,  int mj,  int endlink);
    
    /**
     * Computes the Jacobian matrix expressed in the base frame.
     * 
     * @return jacobian matrix
     */
    Matrix jacobian();
    
    /**
     * Computes the Jacobian matrix expressed in the ref-th frame.
     *
     * @param ref
     *            frame reference
     * @return jacobian matrix
     */
    Matrix jacobian( int ref);
    
    /**
     * Computes the Jacobian time derivative matrix expressed in the base
     * frame.
     * 
     * @return jacobian time derivative matrix
     */
    Matrix jacobianDot();
    
    /**
     * Computes the Jacobian time derivative matrix expressed in the ref-th
     * frame.
     * 
     * @param ref
     *            frame reference
     * @return jacobian time derivative matrix
     */
    Matrix jacobianDot( int ref);
    
    /**
     * Computes the 6x6 inverse Jacobian matrix based on the Damped
     * Least-Squares scheme.
     * 
     * 
     * @param eps
     *            the area of singular region
     * @param lambdaMax
     *            the maximum damping factor
     * @return inverse jacobian matrix
     */
    Matrix jacobianDLSinv( double eps,  double lambdaMax);
    
    /**
     * Computes the solution of the Forward Kinematics Problem.
     * 
     * @return homogeneous transform matrix
     */
    RMatrix4d kine();
    
    /**
     * Computes the solution of the Forward Kinematics Problem for the link j.
     * 
     * @param j
     *            link of forward kinematic
     * @return homogeneous transform matrix
     */
    RMatrix4d kine( int j);
    
    /**
     * Computes the partial derivative of homogeneous transform respect j-th
     * joint.
     * 
     * @param j
     *            joint
     * @return homogeneous partial derivative transform
     */
    RMatrix4d dTdqi( int j);
    
    /*
     * --------------------------------------------- Robot Dynamics
     */
    /**
     * Computes the solution of the Forward Dynamics Problem.
     * 
     * @param q
     *            the vector joints position
     * @param qp
     *            the vector joints velocity
     * @param tau
     *            the vector joints torque
     * @return the vector joints acceleration
     */
    RVector acceleration( RVector q,  RVector qp,  RVector tau);

    /**
     * Computes the solution of the Forward Dynamics Problem.
     * 
     * @param q
     *            array of joints position
     * @param qp
     *            array of joints velocity
     * @param tau
     *            array of joints torque
     * @return the array of joints acceleration
     */
    RVector acceleration( double[] q,  double[] qp,  double[] tau);
    /**
     * Computes the solution of the Forward Dynamics Problem with load applied
     * to the last link.
     * 
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param tauCmd
     *            joints torque
     * @param Fext
     *            load applied at last link
     * @param Next
     *            load applied at last link
     * @return joints acceleration
     */
    RVector acceleration( RVector q,  RVector qp,  RVector tauCmd,  RVector3d Fext,  RVector3d Next);
    /**
     * Computes the solution of the Forward Dynamics Problem with load applied
     * to the last link.
     * 
     * @param q
     *            the array of joints position
     * @param qp
     *            the array of joints velocity
     * @param tauCmd
     *            the array of joints torque
     * @param Fext
     *            the array of the load applied at last link
     * @param Next
     *            the array of the load applied at last link
     * @return the array of joints acceleration
     */
    RVector acceleration( double[] q,  double[] qp,  double[] tauCmd,  double[] Fext,  double[] Next);
    
    /**
     * Computes the solution of the Inverse Dynamics Problem.
     * 
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param qpp
     *            joints acceleration
     * @return joints torque
     */
    RVector torque( RVector q,  RVector qp,  RVector qpp);

    /**
     * Computes the solution of the Inverse Dynamics Problem.
     * 
     * 
     * @param q
     *            the array of joints position
     * @param qp
     *            the array of joints velocity
     * @param qpp
     *            the array of joints acceleration
     * @return the array of joints torque
     */
    RVector torque( double[] q,  double[] qp,  double[] qpp);
    /**
     * Computes the solution of the Inverse Dynamics Problem with load applied
     * to the last link.
     * 
     * @param q
     *            joints position
     * @param qp
     *            joints velocity
     * @param qpp
     *            joints acceleration
     * @param Fext
     *            load applied at last link
     * @param Next
     *            load applied at last link
     * @return joints torque
     */
    RVector torque( RVector q,  RVector qp,  RVector qpp,  RVector3d Fext,  RVector3d Next);
    /**
     * Computes the solution of the Inverse Dynamics Problem with load applied
     * to the last link.
     * 
     * @param q
     *            the array of joints position
     * @param qp
     *            the array of joints velocity
     * @param qpp
     *            the array of joints acceleration
     * @param Fext
     *            the array of the load applied at last link
     * @param Next
     *            the array of the load applied at last link
     * @return the array of joints torque
     */
    RVector torque( double[] q,  double[] qp,  double[] qpp,  double[] Fext,  double[] Next);
    
    /**
     * Computes the torques from the gravity effect.
     * 
     * @return joints torque
     */
    RVector torqueGravity();
    
    /**
     * Computes the torques from the Coriolis and centrifugal effect.
     * 
     * @param qp
     *            joints velocity
     * @return joints torque
     */
    RVector torqueC( RVector qp);
   
    /**
     * Computes the torques from the viscous friction.
     * 
     * @param qp
     *            joints velocity
     * @return joints torque
     */
    RVector torqueViscousFriction( RVector qp);
    
    /**
     * Computes the torques from the Coulomb friction.
     * 
     * @param qp
     *            joints velocity
     * @return joints torque
     */
    RVector torqueCoulombFriction( RVector qp);
    
    /**
     * Computes the inertia matrix.
     * 
     * <p>
     * Call C++ Robot method
     * 
     * <pre>
     * inertia(const ColumnVector &amp;q)
     * </pre>
     * 
     * </p>
     * 
     * @param q
     *            joints position
     * @return intertia matrix
     */
    Matrix inertia( RVector q);
    
    /**
     * Gets gravity vector.
     * @return the gravity vector
     */
    RVector3d getGravity();
    
    /**
     * Sets the gravity vector.
     * @param g the new gravity vector.
     */
    void setGravity( RVector3d g);
    
}
